// Copyright 2017 ETH Zurich and University of Bologna.
// Copyright and related rights are licensed under the Solderpad Hardware
// License, Version 0.51 (the “License”); you may not use this file except in
// compliance with the License.  You may obtain a copy of the License at
// http://solderpad.org/licenses/SHL-0.51. Unless required by applicable law
// or agreed to in writing, software, hardware and materials distributed under
// this License is distributed on an “AS IS” BASIS, WITHOUT WARRANTIES OR
// CONDITIONS OF ANY KIND, either express or implied. See the License for the
// specific language governing permissions and limitations under the License.
/////////////////////////////////////////////////////////
// includes
/////////////////////////////////////////////////////////
#include "mlHomErr.h"
#include "math_fns.h"
#include "bar.h"
#include "utils.h"
#include "bench.h"
/////////////////////////////////////////////////////////
// shared globals
/////////////////////////////////////////////////////////


    

    __attribute__ ((section(".heapsram")))  static const float fv0[400] = { 3.81994295F, 3.30669403F, 6.39751577F,
    8.69755077F, 1.20618272F, 1.32376111F, 5.74598742F, 8.6349411F, 3.34688354F,
    2.50224853F, 1.2741127F, 9.09599304F, 6.47524071F, 3.0908525F, 9.80740738F,
    0.139589429F, 2.33039641F, 7.90400887F, 3.07221794F, 7.63920927F, 9.7196064F,
    7.19499779F, 4.56805754F, 6.52467775F, 1.44638562F, 8.11336613F, 2.20600247F,
    5.91697502F, 2.90632415F, 8.13061714F, 2.53397489F, 8.77460575F, 8.60338783F,
    4.49858904F, 6.94243574F, 2.13343835F, 1.20140541F, 3.87069368F,
    0.907997131F, 6.11449242F, 4.89754772F, 8.05629158F, 7.79883099F,
    5.48923922F, 1.13360369F, 9.6736021F, 7.39435101F, 2.14931798F, 1.04917169F,
    1.03549135F, 1.70854044F, 8.21711826F, 3.41657877F, 7.55680418F, 0.857559F,
    1.94417214F, 5.96305704F, 2.86524963F, 3.2197578F, 1.15129268F, 6.02039051F,
    5.2496295F, 0.737368166F, 8.64953518F, 7.12934971F, 6.84984732F, 7.80683279F,
    8.37815762F, 4.76918745F, 8.49805927F, 9.48979473F, 8.16875839F, 8.99946499F,
    7.9044137F, 9.80366707F, 6.97920895F, 5.66867685F, 5.42141533F, 0.201400638F,
    9.38580608F, 5.28600502F, 4.64609575F, 3.51821637F, 2.09048343F, 4.53706217F,
    2.55300689F, 2.30739212F, 3.30863047F, 5.22878885F, 8.16199303F, 1.03740942F,
    8.01680279F, 0.734394729F, 0.423156619F, 8.15197468F, 1.30348253F,
    4.36203432F, 5.12593746F, 6.40043402F, 9.01374149F, 1.28536427F, 4.01416731F,
    6.44762135F, 6.68933058F, 8.67736149F, 9.16417F, 3.57430339F, 8.61659908F,
    2.53399801F, 9.9039278F, 1.82910717F, 4.72072268F, 1.70567322F, 7.57220316F,
    9.33391F, 6.23286247F, 3.48963881F, 4.84483433F, 0.716863573F, 9.83706665F,
    1.74446023F, 8.38141823F, 7.09788084F, 1.83244455F, 0.814801276F,
    4.09478235F, 0.867367327F, 7.42909908F, 5.70510721F, 4.52644539F,
    6.26119757F, 7.56274509F, 7.89089632F, 6.47334051F, 9.70444489F,
    0.0025147805F, 7.28365517F, 8.32637119F, 6.00919199F, 7.61892F, 9.71707344F,
    3.44486499F, 3.60576868F, 3.04506564F, 3.68224573F, 5.33198643F, 2.86810422F,
    3.53866625F, 4.89306688F, 1.82665408F, 5.55284262F, 3.68002748F, 4.92673206F,
    7.59712172F, 4.72050095F, 4.07322598F, 4.22391033F, 5.51771975F, 2.26589274F,
    0.998106122F, 1.48524952F, 9.6040678F, 6.5597744F, 6.68423843F, 7.30317116F,
    6.31656313F, 2.64872169F, 8.76818943F, 9.8139F, 3.26326442F, 8.03490543F,
    7.22725916F, 9.00202656F, 6.36729431F, 8.80396843F, 3.45804524F, 2.85910177F,
    2.05426574F, 9.74322891F, 2.58305979F, 9.39029884F, 0.33094117F, 2.18298841F,
    9.54368782F, 9.69752693F, 7.34258366F, 4.860147F, 3.7932992F, 4.89306402F,
    2.76974511F, 4.50577068F, 1.42509818F, 2.82376623F, 5.74244642F, 4.45981312F,
    4.20976448F, 8.51812267F, 1.07040596F, 7.72371912F, 1.51304829F, 9.39569092F,
    3.00850868F, 3.41274548F, 8.83163929F, 7.75250435F, 7.21068287F, 1.09512401F,
    4.39689541F, 0.741416097F, 9.52220154F, 6.08722496F, 7.74953175F, 4.1436758F,
    2.38117409F, 0.819834352F, 9.68129444F, 0.822901487F, 9.35880566F,
    6.06513834F, 0.734081686F, 3.35763192F, 4.43963814F, 7.21043682F,
    9.26503563F, 7.94104242F, 1.25679827F, 9.49855232F, 9.3831768F, 5.71027F,
    6.56314087F, 2.04781985F, 0.502598822F, 1.19180024F, 5.44894552F,
    7.11212635F, 9.69584179F, 6.85789156F, 0.471996278F, 4.81503391F,
    7.20404339F, 9.6311245F, 3.86448908F, 8.17976093F, 7.83689117F, 7.43119097F,
    7.90164471F, 1.22269106F, 5.82861948F, 1.94734621F, 0.832351685F,
    3.29994583F, 1.19025767F, 6.33504F, 1.1387459F, 5.1076808F, 3.10467458F,
    1.08879709F, 2.75878572F, 1.51625609F, 4.48335075F, 2.51312304F, 4.67059183F,
    2.19612622F, 3.18344855F, 5.68898201F, 2.23842812F, 0.549617F, 8.70654488F,
    4.7449975F, 5.03779554F, 7.8387351F, 3.03107142F, 3.28392053F, 6.49066544F,
    2.98066F, 9.2887249F, 6.69545841F, 2.67807913F, 9.1022625F, 2.58324623F,
    7.50834227F, 4.51471138F, 6.61464787F, 1.62403691F, 1.74597239F, 3.50757551F,
    1.88788438F, 5.90818691F, 6.54732227F, 9.77792F, 9.82882881F, 1.83512771F,
    3.40832829F, 9.71048355F, 5.17080593F, 5.56617832F, 2.54116678F, 9.76402664F,
    0.28910014F, 7.00168276F, 4.11281204F, 5.46558094F, 7.25913239F, 9.78915787F,
    2.56357217F, 5.9897728F, 0.323252439F, 0.81935966F, 9.14290237F, 4.24043179F,
    5.97782946F, 9.96217251F, 2.31157708F, 7.8056F, 7.82927799F, 5.60166502F,
    3.35788226F, 7.05825806F, 0.66223079F, 1.80157065F, 4.55646563F, 7.52401638F,
    6.02525043F, 8.84383488F, 1.74871767F, 7.02806854F, 7.86188745F, 8.74725246F,
    2.79992F, 2.63027978F, 5.19991875F, 8.25669193F, 0.224559829F, 4.15020227F,
    3.78346419F, 9.83358765F, 6.52887297F, 7.78960085F, 0.431559205F,
    5.13839483F, 9.77352619F, 1.35473096F, 3.00135064F, 1.94097638F, 2.18482065F,
    3.1533041F, 5.50896F, 1.2114321F, 2.73187804F, 8.47578621F, 4.82433748F,
    4.87874079F, 3.44894218F, 3.93672752F, 4.30371284F, 3.89207363F, 9.43695F,
    5.80956697F, 2.61447763F, 5.21682835F, 2.40744781F, 8.19742107F, 8.69043446F,
    4.13594103F, 4.95426941F, 0.863913357F, 2.57841444F, 7.15571165F,
    5.27504539F, 2.31878781F, 4.32288694F, 9.08087444F, 9.5610714F, 0.393213779F,
    8.74852753F, 5.17191219F, 3.7630291F, 8.34237099F, 2.63563895F, 0.550342381F,
    3.96306849F, 2.37854314F, 0.0972838253F, 1.11429203F, 3.71485758F,
    3.61351466F, 3.03973746F, 4.84496593F, 5.82391357F, 9.33076668F, 4.57339811F,
    5.28623343F, 4.07505846F, 5.9545536F, 2.49791646F, 0.916460156F, 9.74848366F,
    4.67191648F, 5.22091389F, 5.10168028F };

    __attribute__ ((section(".heapsram"))) static const float fv1[400] = { 5.46120691F, 8.67507935F, 5.20149946F,
    3.21667433F, 5.06526184F, 8.23287392F, 2.78341866F, 4.33575869F, 4.47248507F,
    2.15086508F, 9.56404495F, 9.00784302F, 4.10451365F, 4.90767241F,
    0.141741678F, 1.03942382F, 7.11035872F, 4.17596292F, 4.10651159F,
    0.132129446F, 1.94768596F, 5.84430265F, 5.2689743F, 3.04336143F,
    0.795984507F, 2.77241F, 3.70748925F, 9.98895931F, 4.54478455F, 3.66679978F,
    7.62751293F, 5.36315489F, 6.5314455F, 3.04432559F, 3.48048449F, 3.00382638F,
    1.35541642F, 2.1040132F, 5.79528856F, 7.41677F, 3.66044402F, 6.95215464F,
    4.17089415F, 4.03894806F, 9.33655548F, 8.112607F, 7.65085936F, 8.81519222F,
    2.9513433F, 1.36225808F, 6.07025146F, 0.351477861F, 9.74055386F, 7.06550455F,
    1.4720571F, 0.823607922F, 8.12792873F, 5.89299297F, 7.30732775F, 5.62524271F,
    9.07204914F, 2.57973F, 6.36308241F, 2.30937457F, 4.07567167F, 4.29183245F,
    3.35022259F, 3.37107539F, 2.21471786F, 5.31217384F, 1.96841931F, 1.11074972F,
    2.04722786F, 5.32955933F, 6.69584703F, 2.29313064F, 8.48539829F, 1.33272624F,
    3.32749081F, 1.75656557F, 5.6752615F, 8.61337852F, 6.19005203F, 4.90300846F,
    0.604074419F, 6.28612661F, 0.744509F, 3.70698953F, 9.26716423F, 2.54720426F,
    2.74726963F, 6.75874472F, 3.16395402F, 0.608026505F, 5.77129459F,
    6.93361282F, 4.57821131F, 6.34421206F, 6.07810831F, 9.48091316F, 3.99573541F,
    4.84784269F, 6.88964796F, 4.28378248F, 3.87202239F, 2.03364277F, 5.4358449F,
    8.48714828F, 5.06083822F, 3.57479286F, 0.65060991F, 2.94477916F,
    0.0221841596F, 0.136495799F, 9.81480694F, 4.02283239F, 6.68476534F,
    6.48966885F, 4.76232815F, 0.848488033F, 4.69308853F, 6.31143904F, 4.03887F,
    2.34439135F, 4.07491F, 2.09615779F, 0.880912781F, 7.90170479F, 0.0803481F,
    4.47838688F, 1.39709592F, 6.59452534F, 0.637088954F, 1.73229539F,
    3.53470182F, 5.47727919F, 6.71049738F, 1.96881282F, 4.88886786F, 5.63562965F,
    2.737643F, 9.91621113F, 0.246617571F, 8.74428F, 2.18569493F, 7.10883F,
    0.500896335F, 8.19873428F, 4.21434546F, 8.87994099F, 3.95110822F,
    4.02664328F, 9.33608913F, 0.574728549F, 0.615705311F, 2.53704023F,
    9.33576775F, 6.90303F, 9.63939857F, 6.87137413F, 1.28075385F, 3.13568902F,
    5.71924F, 8.71583843F, 9.86707592F, 7.5568223F, 5.95021F, 7.98547935F,
    5.75501442F, 9.09518433F, 7.96208143F, 4.18196583F, 3.66633463F, 5.17967796F,
    5.98222494F, 1.52824116F, 2.21326208F, 0.563391268F, 4.17083406F,
    6.31446743F, 0.926538169F, 7.30662966F, 0.81325078F, 3.33544874F, 9.75465F,
    7.8167038F, 3.42427516F, 8.77025223F, 1.13095081F, 9.37776F, 6.55875778F,
    8.90794563F, 6.68296766F, 4.63185453F, 0.441807F, 3.98923945F, 3.57656765F,
    5.82027817F, 5.9353447F, 4.33008146F, 9.21142387F, 3.72546172F, 8.19428539F,
    6.63451338F, 6.25897217F, 5.0901618F, 7.4606452F, 1.69700623F, 2.57604027F,
    4.33597755F, 1.17186844F, 5.07283783F, 4.19948578F, 0.0191385224F,
    1.89565158F, 2.09496284F, 1.58784962F, 1.38286662F, 4.7025342F, 1.36129224F,
    2.81322956F, 7.23808908F, 8.34421635F, 0.00403929781F, 7.66006756F,
    5.32372475F, 5.22099733F, 1.18330514F, 8.81678104F, 9.40350533F, 5.32107401F,
    7.66374159F, 3.04726458F, 5.68803501F, 1.98071229F, 3.51860619F, 7.84271669F,
    6.82111216F, 3.37518191F, 9.71256256F, 7.78693295F, 5.35573387F, 7.82355595F,
    8.15700817F, 3.57771F, 6.67836332F, 7.21723318F, 7.83939886F, 0.249831095F,
    4.7841053F, 7.14212751F, 2.02928019F, 6.47702265F, 0.398544341F, 8.84154129F,
    7.73490143F, 1.65195489F, 1.7425375F, 0.4156F, 0.989879072F, 6.9559269F,
    8.05454159F, 7.60897493F, 9.06170082F, 1.88401949F, 1.6994282F, 3.9096179F,
    6.48410892F, 5.08647394F, 4.52009678F, 5.53885078F, 6.0966239F, 9.1142149F,
    1.12666547F, 5.20537853F, 8.45338F, 9.29678726F, 3.92909479F, 5.54197598F,
    9.74499416F, 0.608571589F, 4.86052084F, 3.01884842F, 5.38886309F,
    4.62432051F, 0.368984699F, 1.55414295F, 9.34004879F, 8.22838F, 6.25555372F,
    7.65791655F, 4.78058815F, 2.13059163F, 0.610396743F, 7.4443512F, 2.6404376F,
    0.318843752F, 3.4258945F, 4.87193298F, 8.10996342F, 6.97079706F, 7.54670954F,
    9.55272293F, 5.26363373F, 0.708496511F, 5.70628595F, 2.1798203F, 8.20435238F,
    9.48739433F, 1.05263567F, 3.53005981F, 2.43074846F, 4.76536512F, 4.36680937F,
    5.00956202F, 9.07940102F, 8.50595284F, 3.07144928F, 2.75778341F, 8.81400681F,
    2.20475197F, 5.96988678F, 4.77590895F, 8.62470436F, 8.84685421F, 5.72273588F,
    7.24148655F, 8.83590698F, 6.26323891F, 9.13224792F, 9.50937748F, 5.83877182F,
    2.20805407F, 5.09606647F, 6.41938972F, 7.23281908F, 5.85914469F, 8.46591949F,
    5.71872282F, 3.39596987F, 7.86945057F, 4.11172104F, 4.56353807F, 8.25829F,
    7.10952568F, 0.771528542F, 2.95444155F, 9.52907848F, 5.65224886F,
    0.840792835F, 6.50535488F, 2.32446F, 4.60725403F, 4.29107714F, 2.91472101F,
    6.86410904F, 9.4273777F, 4.68843651F, 0.708094954F, 4.79111814F, 2.92325234F,
    2.82684469F, 0.847125828F, 4.95976639F, 8.7906332F, 1.43146646F, 5.87328196F,
    9.60985851F, 5.04590273F, 0.305774391F, 7.91194582F, 1.59907365F,
    2.80242014F, 6.36649418F, 2.5615797F, 3.85117483F, 8.88559151F, 2.82526016F,
    7.36446905F, 0.876706362F, 1.94540644F, 3.4910984F, 7.91022921F, 1.39463317F,
    3.25219631F, 8.061759F, 6.20720148F, 3.11999035F, 9.77121449F, 1.66359413F,
    1.46628654F, 8.58532619F, 7.41906595F, 7.18331289F, 0.346298784F,
    8.13638687F, 6.05578566F, 4.71069765F, 4.18074608F, 4.35130215F };

    __attribute__ ((section(".heapsram"))) static const float fv2[36] = { 4.4693017F, 7.77953625F, 8.14498901F, 8.80405F,
    9.59094906F, 5.47567415F, 9.13130665F, 1.35224F, 7.48299885F, 0.61214155F,
    9.57826519F, 1.33492291F, 2.51902604F, 1.20082235F, 2.04344678F, 1.65944946F,
    2.41911435F, 4.49521494F, 1.79205215F, 7.3779F, 6.84530592F, 4.28945208F,
    5.03651237F, 3.28347564F, 8.84074116F, 7.65366459F, 7.45727062F, 3.56547356F,
    1.51294565F, 9.37439251F, 8.37396049F, 7.26953745F, 4.1659832F, 9.58267403F,
    4.92166805F, 8.31483459F };

    __attribute__ ((section(".heapsram"))) static const float fv3[8] = { 20749.9062F, 20749.4922F, 100889.633F,
    100885.622F, 11047.5176F, 11047.2969F, 7426.02344F, 7425.875F };


/////////////////////////////////////////////////////////
// subfunctions
/////////////////////////////////////////////////////////


/*
 * Arguments    : const float a[50]
 *                float b[100]
 * Return Type  : void
 */
void repmat(const float a[50], float b[100])
{
  int jcol;
  int ibmat;
  int itilerow;
  for (jcol = 0; jcol < 50; jcol++) {
    ibmat = jcol << 1;
    for (itilerow = 0; itilerow < 2; itilerow++) {
      b[ibmat + itilerow] = a[jcol];
    }
  }
}

/*
 * Arguments    : const float H[9]
 *                const float x1[100]
 *                const float x2[100]
 * Return Type  : float
 */
float mlHomErr(const float H[9], const float x1[100], const float x2[100])
{
  float err;
  int rtemp;
  int k;
  float tmp[150];
  int r1;
  float fv4[50];
  float Hx1[100];
  float B[150];
  float A[9];
  int r2;
  int r3;
  float maxval;
  float a21;
  float invHx2[100];
  float b_Hx1;
  float b_invHx2;

  /*  mlHomErr */
  for (rtemp = 0; rtemp < 3; rtemp++) {
    for (k = 0; k < 50; k++) {
      tmp[rtemp + 3 * k]  = H[rtemp]     * x1[    (k<<1)];
      tmp[rtemp + 3 * k] += H[rtemp + 3] * x1[1 + (k<<1)];
      tmp[rtemp + 3 * k] += H[rtemp + 6];
    }
  }

  for (rtemp = 0; rtemp < 50; rtemp++) {
    fv4[rtemp] = fDiv(1.0F, tmp[2 + 3 * rtemp]);
  }

  repmat(fv4, Hx1);
  for (rtemp = 0; rtemp < 50; rtemp++) {
    for (k = 0; k < 2; k++) {
      Hx1[k + (rtemp << 1)] *= tmp[k + 3 * rtemp];
      B[k + 3 * rtemp] = x2[k + (rtemp << 1)];
    }

    B[2 + 3 * rtemp] = 1.0F;
  }

  for (rtemp = 0; rtemp < 9; rtemp++) {
    A[rtemp] = H[rtemp];
  }

  r1 = 0;
  r2 = 1;
  r3 = 2;
  maxval = (real32_T)fAbs(H[0]);
  a21 = (real32_T)fAbs(H[1]);
  if (a21 > maxval) {
    maxval = a21;
    r1 = 1;
    r2 = 0;
  }

  if ((real32_T)fAbs(H[2]) > maxval) {
    r1 = 2;
    r2 = 1;
    r3 = 0;
  }

  A[r2] = fDiv(H[r2], H[r1]);
  A[r3] = fDiv(A[r3], A[r1]);
  A[3 + r2] -= A[r2] * A[3 + r1];
  A[3 + r3] -= A[r3] * A[3 + r1];
  A[6 + r2] -= A[r2] * A[6 + r1];
  A[6 + r3] -= A[r3] * A[6 + r1];
  if ((real32_T)fAbs(A[3 + r3]) > (real32_T)fAbs(A[3 + r2])) {
    rtemp = r2;
    r2 = r3;
    r3 = rtemp;
  }

  A[3 + r3] = fDiv(A[3 + r3], A[3 + r2]);
  A[6 + r3] -= A[3 + r3] * A[6 + r2];
  for (k = 0; k < 50; k++) {
    tmp[3 * k] = B[r1 + 3 * k];
    tmp[1 + 3 * k] = B[r2 + 3 * k] - tmp[3 * k] * A[r2];
    tmp[2 + 3 * k] = (B[r3 + 3 * k] - tmp[3 * k] * A[r3]) - tmp[1 + 3 * k] * A[3  + r3];
    tmp[2 + 3 * k] = fDiv(tmp[2 + 3 * k], A[6 + r3]);
    tmp[3 * k] -= tmp[2 + 3 * k] * A[6 + r1];
    tmp[1 + 3 * k] -= tmp[2 + 3 * k] * A[6 + r2];
    tmp[1 + 3 * k] = fDiv(tmp[1 + 3 * k], A[3 + r2]);
    tmp[3 * k] -= tmp[1 + 3 * k] * A[3 + r1];
    tmp[3 * k] = fDiv(tmp[3 * k], A[r1]);
    fv4[k] = fDiv(1.0F, tmp[2 + 3 * k]);
  }

  repmat(fv4, invHx2);
  for (rtemp = 0; rtemp < 50; rtemp++) {
    for (k = 0; k < 2; k++) {
      invHx2[k + (rtemp << 1)] *= tmp[k + 3 * rtemp];
    }
  }

  err = 0.0F;
  for (rtemp = 0; rtemp < 100; rtemp++) {
    b_Hx1 = Hx1[rtemp] - x2[rtemp];
    b_invHx2 = invHx2[rtemp] - x1[rtemp];
    err += b_Hx1 * b_Hx1 + b_invHx2 * b_invHx2;
  }

  return err;
}


/////////////////////////////////////////////////////////
// main testing function 
/////////////////////////////////////////////////////////
int main(int argc, const char * const argv[])
{
  (void)argc;
  (void)argv;

  int it;

  boolean_T pass, flag;
  int coreid;
  float err[KERNEL_ITS];
  int i0;

  /////////////////////////////////////////////////////////
  // main test loop 
  // each core loops over a kernel instance
  /////////////////////////////////////////////////////////

  coreid = get_core_id();

  printf("starting %d kernel iterations... (coreid = %d)\n",KERNEL_ITS,coreid);

  if (coreid>3)
    coreid=coreid-4;

  synch_barrier();

  perf_begin();

  for(it = 0; it < KERNEL_ITS; it++)
  {

    err[it] = mlHomErr(*(float (*)[9])&fv2[9 * coreid], *(float (*)[100])&fv1[100 * coreid], *(float (*)[100])&fv0[100 * coreid]);
  }  

  synch_barrier();
  
  perf_end();

  /////////////////////////////////////////////////////////
  // check results
  /////////////////////////////////////////////////////////
  pass = true;
  for(it = 0; it < KERNEL_ITS; it++)
    {
      flag = (err[it] <= fv3[0 + (coreid << 1)]) && (err[it] >= fv3[1 + (coreid << 1)]);
      pass = pass & flag;
    
      printErrors(!flag, it, err[it] ,fv3[0 + (coreid << 1)] ,fv3[1 + (coreid << 1)]);
    }

  flagPassFail(pass, get_core_id());

/////////////////////////////////////////////////////////
// synchronize and exit
/////////////////////////////////////////////////////////

  return !pass;
}

